"""
最终pnp解算脚本，在获得雷达中四点和相机中四点后使用
"""
from camera_utils.camera import read_yaml
from camera_utils.config import CALIB_DIR,CALIB_MODE,CAMERA_TYPE
import numpy as np
import cv2

if __name__ == "__main__":
    dir = CALIB_DIR
    ct = 0 # which camera type
    if CALIB_MODE == 1:
        ct = CAMERA_TYPE
    MODE = CALIB_MODE
    num = 5 # how many photos
    _,K_0,C_0,_,_ = read_yaml(ct)
    cps = np.zeros((4 * num, 2))
    lps = np.zeros((4 * num, 3))


    for i in range(num):
        if MODE == 2:
            cps[int(i * 4):int((i + 1) * 4)] = np.loadtxt(f"{dir}/{ct}_{i * 2 + ct}_corner.txt", dtype=object,
                                                          delimiter=' ')[:, :2].astype(np.float32).reshape(-1, 2)
        else:
            cps[int(i * 4):int((i + 1) * 4)] = np.loadtxt(f"{dir}/{ct}_{i}_corner.txt", dtype=object,
                                                          delimiter=' ')[:, :2].astype(np.float32).reshape(-1, 2)
    for i in range(num):
        if MODE == 2:
            lps[int(i * 4):int((i + 1) * 4)] = np.loadtxt(f"{dir}/{0}_{i*2+0}_cors.txt", delimiter=' ').reshape(-1, 3)
        else:
            lps[int(i * 4):int((i + 1) * 4)] = np.loadtxt(f"{dir}/{0}_{i}_cors.txt", delimiter=' ').reshape(-1,3)
    lps = np.float32(lps).reshape(-1, 3)
    cps = np.float32(cps).reshape(-1, 2)
    flag, rvec, tvec = cv2.solvePnP(lps, cps, K_0, C_0)
    R = cv2.Rodrigues(rvec)[0]
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = tvec.reshape(-1)
    ims = cv2.projectPoints(lps, rvec, tvec, K_0, C_0)[0].reshape(-1, 2)

    for i in range(4):
        print("%0.9f, %0.9f, %0.9f, %0.9f," % tuple(T[i]))
    n = np.linalg.norm((ims - cps), axis=1)
    print(f"error is {n.sum() / cps.shape[0]:.5f}")

